#include <string.h>
#include "ZComDef.h"
#include "OSAL.h"
//#include "hal_uart.h"
#include "OSAL_Memory.h"
#include "OnBoard.h"
#include "ZGlobals.h"
#include "osal_Nv.h"
#include "bdb_interface.h"

#include "zcl.h"
#include "ZDObject.h"

#include "mfg.h"
#include "cmd.h"
#include "cmd_uart.h"
#include "cmd_nwk.h"


/* MACRO */
#define    ZDP_SEQ_TABLE_SIZE   16
#define    ZDP_SEQ_TABLE_LIFE   10

/* TYPEDEF */
typedef uint8 (*pfnCmdZdpIndCB_t)(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);

typedef struct
{
    uint16 cmd;
    pfnCmdZdpIndCB_t cb;
}cmdZdpIndItem_t;

typedef struct
{
  uint16 cmd;
  uint16 nodeId;
  uint8  handle;
}cmdZdpCnfParam_t;

typedef struct
{
  uint8 handle;
  uint8 life;
}cmdZdpSeqTable_t;

/* LOCAL FUNCTION */
void cmdZdp_SendCnf(uint8 status, uint8 endpoint, uint8 transID, void* cnfParam);
void cmdZdp_SeqPush(uint8 handle);
bool cmdZdp_SeqCmp(uint8 handle);

void cmdZdp_CnfOutput(uint16 cmd, uint16 nodeId, uint8 status, uint8 handle);
void cmdZdp_CmdOutput(uint16 cmd, uint16 nodeId, uint8 status, uint8 handle, uint8 len, uint8* data);

//ZDP Request
uint8 cmdZdp_NwkAddrReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_IeeeAddrReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_SimpleDescReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_ActiveEpReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_BindReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_UnbindReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_BindTableReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);
uint8 cmdZdp_LeaveReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle);

/* extern variable */
extern uint8 cmdPktTaskId;

/* local variable */
cmdZdpIndItem_t cmdZdp_IndTable[] =
{
  {ZDP_NWK_ADDR,cmdZdp_NwkAddrReq},
  {ZDP_IEEE_ADDR,cmdZdp_IeeeAddrReq},
  {ZDP_SIMPLE_DESC,cmdZdp_SimpleDescReq},
  {ZDP_ACTIVE_EP,cmdZdp_ActiveEpReq},
  {ZDP_BIND,cmdZdp_BindReq},
  {ZDP_UNBIND,cmdZdp_UnbindReq},
  {ZDP_BIND_TABLE,cmdZdp_BindTableReq},
  {ZDP_LEAVE,cmdZdp_LeaveReq},
  {0xFFFF,NULL},
};

cmdZdpSeqTable_t cmdZdpSeqTable[ZDP_SEQ_TABLE_SIZE];
uint8 cmdZdpSeqIdx = 0;

/**************************************************
                   Function
*/

/**************************************************

*/
void cmdZdp_CmdInput(uint8 cmdSeq, uint8 cmdClass, uint8 cmdLen, uint8* cmdData)
{
  uint16 cmd = BUILD_UINT16(cmdData[0],cmdData[1]);
  uint16 dest = BUILD_UINT16(cmdData[2],cmdData[3]);
  uint8  ack[2];
  uint8  n;
  uint8  status = ZFailure;
  uint8  handle = 0;
  for(n=0; n<(sizeof(cmdZdp_IndTable)/sizeof(cmdZdpIndItem_t)); n++)
  {
    if(cmdZdp_IndTable[n].cmd == cmd)
    {
      if(cmdZdp_IndTable[n].cb)
      {
        status = cmdZdp_IndTable[n].cb( cmd, dest, cmdLen-4, cmdData+4, &handle );
        cmdZdp_SeqPush(handle);
      }
      break;
    }
    else if(cmdZdp_IndTable[n].cmd == 0xFFFF)
    {
      break;
    }
  }
  ack[0] = status;
  ack[1] = handle;
  cmd_UartAckSend(cmdSeq,cmdClass,2,ack);
}

void cmdZdp_SendCnf(uint8 status, uint8 endpoint, uint8 transID, void* cnfParam)
{
  if( cnfParam )
  {
    cmdZdpCnfParam_t *param = cnfParam;
    cmdZdp_CnfOutput( param->cmd, param->nodeId, status, param->handle);
    MFG_free(cnfParam);
  }
}

void cmdZdp_CnfOutput(uint16 cmd, uint16 nodeId, uint8 status, uint8 handle)
{
  uint8 buf[8];
  buf[0] = cmd_UartGetSeqNum();
  buf[1] = CMD_CLASS_NWK;
  buf[2] = LO_UINT16(cmd);
  buf[3] = HI_UINT16(cmd);
  buf[4] = LO_UINT16(nodeId);
  buf[5] = HI_UINT16(nodeId);
  buf[6] = status;
  buf[7] = handle;
  cmd_UartTxSend( 8, buf );
}

void cmdZdp_CmdOutput(uint16 cmd, uint16 nodeId, uint8 status, uint8 handle, uint8 len, uint8* data)
{
  //2+2+1+1+ (80-2)
  uint8 buf[86];
  buf[0] = cmd_UartGetSeqNum();
  buf[1] = CMD_CLASS_NWK_INFO;
  buf[2] = LO_UINT16(cmd);
  buf[3] = HI_UINT16(cmd);
  buf[4] = LO_UINT16(nodeId);
  buf[5] = HI_UINT16(nodeId);
  buf[6] = status;
  buf[7] = handle;
  memcpy( buf+8, data, len );
  cmd_UartTxSend( len+8, buf );
}

void cmdZdp_SeqPush(uint8 handle)
{
  cmdZdpSeqTable[cmdZdpSeqIdx].handle = handle;
  cmdZdpSeqTable[cmdZdpSeqIdx].life = ZDP_SEQ_TABLE_LIFE;
  cmdZdpSeqIdx += 1;
  cmdZdpSeqIdx &= (ZDP_SEQ_TABLE_SIZE - 1);
}

bool cmdZdp_SeqCmp(uint8 handle)
{
  uint8 n;
  for( n=0; n <ZDP_SEQ_TABLE_SIZE; n++ )
  {
    if( (cmdZdpSeqTable[n].life > 0) &&
       (cmdZdpSeqTable[n].handle == handle) )
    {
      return true;
    }
  }
  return false;
}

void cmdZdp_SeqTablePoll(void)
{
  uint8 n;
  for( n=0; n <ZDP_SEQ_TABLE_SIZE; n++ )
  {
    if(cmdZdpSeqTable[cmdZdpSeqIdx].life > 0)
    {
      cmdZdpSeqTable[cmdZdpSeqIdx].life--;
    }
  }
}

/* ZDP command */
/*******************************************************
                ZDP_NWK_ADDR
*/
uint8 cmdZdp_NwkAddrReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  uint8 status = ZFailure;
  if((dest != 0xFFFD) || (len < 10))
  {
    return status;
  }
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    uint8 type = data[0];
    uint8 startIndex = data[1];
    uint8 ieeeAddr[8];
    memcpy( ieeeAddr, data + 2, 8 );
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_NwkAddrReq( ieeeAddr, type, startIndex, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_NwkAddrRsp(ZDO_NwkIEEEAddrResp_t rsp)
{
  if(cmdZdp_SeqCmp(rsp.transSeq))
  {
    uint8 len = 0;
    uint8 buf[100];
    uint8 *p = buf;
    uint8 n;
    //IEEE
    memcpy( p, rsp.extAddr, 8 );
    p += 8;
    len += 8;
    //start index & assoc num
    *p ++ = rsp.startIndex;
    *p ++ = rsp.numAssocDevs;
    len += 2;
    //assoc list
    for( n=0; n<rsp.numAssocDevs; n++ )
    {
      *p++ = LO_UINT16(rsp.devList[n]);
      *p++ = HI_UINT16(rsp.devList[n]);
      len += 2;
    }
    cmdZdp_CmdOutput( (ZDP_NWK_ADDR | 0x8000), rsp.nwkAddr, rsp.status, rsp.transSeq, len, buf);
  }
}

/*******************************************************
                ZDP_IEEE_ADDR
*/
uint8 cmdZdp_IeeeAddrReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    uint16 nwkAddr = dest;
    uint8 type = data[0];
    uint8 startIndex = data[1];
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_IEEEAddrReq( nwkAddr, type, startIndex, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
  }
  return (uint8)status;
}

void cmdZdp_IeeeAddrRsp(ZDO_NwkIEEEAddrResp_t rsp)
{
  if(cmdZdp_SeqCmp(rsp.transSeq))
  {
    uint8 len = 0;
    uint8 buf[100];
    uint8 *p = buf;
    uint8 n;
    //IEEE
    memcpy( p, rsp.extAddr, 8 );
    p += 8;
    len += 8;
    //start index & assoc num
    *p ++ = rsp.startIndex;
    *p ++ = rsp.numAssocDevs;
    len += 2;
    //assoc list
    for( n=0; n<rsp.numAssocDevs; n++ )
    {
      *p++ = LO_UINT16(rsp.devList[n]);
      *p++ = HI_UINT16(rsp.devList[n]);
      len += 2;
    }
    cmdZdp_CmdOutput( (ZDP_IEEE_ADDR | 0x8000), rsp.nwkAddr, rsp.status, rsp.transSeq, len, buf);
  }
}

/*******************************************************
               ZDP_SIMPLE_DESC
*/
uint8 cmdZdp_SimpleDescReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  if(len < 1)
  {
    return ZFailure;
  }
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    zAddrType_t dstAddr;
    dstAddr.addrMode = Addr16Bit;
    dstAddr.addr.shortAddr = dest;
    uint8 endpoint = data[0];
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_SimpleDescReq( &dstAddr, dest, endpoint, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_SimpleDescRsp(ZDO_SimpleDescRsp_t rsp)
{
  if(cmdZdp_SeqCmp(rsp.transSeq))
  {
    uint8 len = 0;
    uint8 buf[80];
    if(rsp.status == ZDP_SUCCESS)
    {
      uint8 *p = buf + 6;
      uint8  i;
      len = 8 + rsp.simpleDesc.AppNumInClusters*2 + rsp.simpleDesc.AppNumOutClusters*2;
      buf[0] = rsp.simpleDesc.EndPoint;
      buf[1] = LO_UINT16(rsp.simpleDesc.AppProfId);
      buf[2] = HI_UINT16(rsp.simpleDesc.AppProfId);
      buf[3] = LO_UINT16(rsp.simpleDesc.AppDeviceId);
      buf[4] = HI_UINT16(rsp.simpleDesc.AppDeviceId);
      buf[5] = rsp.simpleDesc.AppDevVer;
      *p++ = rsp.simpleDesc.AppNumInClusters;
      for( i=0 ;i < rsp.simpleDesc.AppNumInClusters; i++ )
      {
        *p++ = LO_UINT16(rsp.simpleDesc.pAppInClusterList[i]);
        *p++ = HI_UINT16(rsp.simpleDesc.pAppInClusterList[i]);
      }
      *p++ = rsp.simpleDesc.AppNumOutClusters;
      for( i=0 ; i < rsp.simpleDesc.AppNumOutClusters; i++ )
      {
        *p++ = LO_UINT16(rsp.simpleDesc.pAppOutClusterList[i]);
        *p++ = HI_UINT16(rsp.simpleDesc.pAppOutClusterList[i]);
      }
    }
    cmdZdp_CmdOutput( (ZDP_SIMPLE_DESC | 0x8000), rsp.nwkAddr, rsp.status, rsp.transSeq, len, buf);
  }
}

/*******************************************************
               ZDP_ACTIVE_EP
*/
uint8 cmdZdp_ActiveEpReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    zAddrType_t dstAddr;
    dstAddr.addrMode = Addr16Bit;
    dstAddr.addr.shortAddr = dest;
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_ActiveEPReq( &dstAddr, dest, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_ActiveEpRsp(ZDO_ActiveEndpointRsp_t* pRsp)
{
  if(cmdZdp_SeqCmp(pRsp->transSeq))
  {
    uint8 len = 0;
    uint8 buf[80];
    if(pRsp->status == ZDP_SUCCESS)
    {
      uint8 *p = buf + 1;
      len = 1 + pRsp->cnt;
      buf[0] = pRsp->cnt;
      memcpy( p, pRsp->epList, pRsp->cnt );
      cmdZdp_CmdOutput( (ZDP_ACTIVE_EP | 0x8000), pRsp->nwkAddr, pRsp->status, pRsp->transSeq, len, buf);
    }
  }
}

/*******************************************************
               ZDP_BIND
*/
uint8 cmdZdp_BindReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  if(len < 20)
  {
    return ZFailure;
  }
  uint16 cluster = BUILD_UINT16(data[0], data[1]);
  uint8 *srcDevSn = data+2;
  uint8 *dstDevSn = srcDevSn+9;
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    zAddrType_t dstAddr;
    dstAddr.addrMode = Addr16Bit;
    dstAddr.addr.shortAddr = dest;
    //set src
    uint8 srcAddr[8];
    byte srcEP = srcDevSn[0];
    memcpy( srcAddr, srcDevSn+1, 8);
    //set bind dst
    zAddrType_t bindAddr;
    byte bindEP = dstDevSn[0];
    if(bindEP == 0xFF)
    {
      bindAddr.addrMode = AddrGroup;
      bindAddr.addr.shortAddr = BUILD_UINT16( dstDevSn[1], dstDevSn[2] );
    }
    else
    {
      if(bindEP == 0)
      {
        bindEP = 1;
        memcpy( bindAddr.addr.extAddr, NLME_GetExtAddr( ), 8 );
      }
      else
      {
        memcpy( bindAddr.addr.extAddr, dstDevSn+1, 8 );
      }
      bindAddr.addrMode = Addr64Bit;
      
    }
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_BindReq( &dstAddr, srcAddr, srcEP, cluster , &bindAddr, bindEP, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_BindRsp(uint16 nwkAddr, uint8 seqNum, uint8 status)
{
  if(cmdZdp_SeqCmp(seqNum))
  {
    cmdZdp_CmdOutput( (ZDP_BIND | 0x8000), nwkAddr, status, seqNum, 0, NULL);
  }
}

/*******************************************************
               ZDP_Unind
*/
uint8 cmdZdp_UnbindReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  if(len < 20)
  {
    return ZFailure;
  }
  uint16 cluster = BUILD_UINT16(data[0], data[1]);
  uint8 *srcDevSn = data+2;
  uint8 *dstDevSn = srcDevSn+9;
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    zAddrType_t dstAddr;
    dstAddr.addrMode = Addr16Bit;
    dstAddr.addr.shortAddr = dest;
    //set src
    uint8 srcAddr[8];
    memcpy( srcAddr, srcDevSn, 8);
    byte srcEP = srcDevSn[8];
    //set bind dst
    zAddrType_t bindAddr;
    bindAddr.addrMode = Addr64Bit;
    memcpy( bindAddr.addr.extAddr, srcDevSn, 8 );
    byte bindEP = dstDevSn[8];
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_UnbindReq( &dstAddr, srcAddr, srcEP, cluster , &bindAddr, bindEP, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_UnbindRsp(uint16 nwkAddr, uint8 seqNum, uint8 status)
{
  if(cmdZdp_SeqCmp(seqNum))
  {
    cmdZdp_CmdOutput( (ZDP_UNBIND | 0x8000), nwkAddr, status, seqNum, 0, NULL);
  }
}

/*******************************************************
               ZDP_BIND_TABLE
*/
uint8 cmdZdp_BindTableReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  if(!len)
  {
    return ZFailure;
  }
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    zAddrType_t dstAddr;
    dstAddr.addrMode = Addr16Bit;
    dstAddr.addr.shortAddr = dest;
    uint8 startIndex = data[0];
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_MgmtBindReq( &dstAddr, startIndex, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_BindTableRsp(uint16 nwkAddr, ZDO_MgmtBindRsp_t* pRsp)
{
  if(cmdZdp_SeqCmp(pRsp->transSeq))
  {
    uint8 len = 3;
    uint8 buf[100];
    uint8 *p = buf + 3;
    uint8 n;
    buf[0] = pRsp->bindingCount;
    buf[1] = pRsp->startIndex;
    buf[2] = pRsp->bindingListCount;
    for(n=0; n<pRsp->bindingListCount; n++)
    {
      apsBindingItem_t *pList = &pRsp->list[n];
      //cluster
      *p++ = LO_UINT16(pList->clusterID);
      *p++ = HI_UINT16(pList->clusterID);
      len += 2;
      //src MAC
      memcpy(p, pList->srcAddr, 8);
      p += 8;
      len += 8;
      //src EP
      *p++ = pList->srcEP;
      len += 1;
      //dest addr
      if(pList->dstAddr.addrMode == Addr64Bit)
      {
        memcpy( p, pList->dstAddr.addr.extAddr, 8 );
      }
      else
      {
        memset( p, 0, 8);
        p[0] = LO_UINT16(pList->dstAddr.addr.shortAddr);
        p[1] = HI_UINT16(pList->dstAddr.addr.shortAddr);
      }
      p += 8;
      len += 8;
      //dest EP
      *p++ = pList->dstEP;
      len += 1;
    }
    cmdZdp_CmdOutput( (ZDP_BIND_TABLE | 0x8000), nwkAddr, pRsp->status, pRsp->transSeq, len, buf );
  }
}

/*******************************************************
               ZDP_LEAVE
*/
uint8 cmdZdp_LeaveReq(uint16 cmd, uint16 dest, uint8 len, uint8 *data, uint8* handle)
{
  if(len < 10)
  {
    return ZFailure;
  }
  uint8 status = ZFailure;
  cmdZdpCnfParam_t *cnfParam = MFG_malloc(sizeof(cmdZdpCnfParam_t));
  if( cnfParam )
  {
    zAddrType_t dstAddr;
    dstAddr.addrMode = Addr16Bit;
    dstAddr.addr.shortAddr = dest;
    uint8 extAddr[8];
    memcpy( extAddr, data, 8 );
    uint8 rejoin = data[8];
    uint8 removeChildren = data[9];
    // Set confirm
    ZDP_SetSendConfirm( cmdZdp_SendCnf, cnfParam );
    cnfParam->handle = ZDP_SeqNum;
    // send command
    status = ZDP_MgmtLeaveReq( &dstAddr, extAddr, removeChildren, rejoin, 0 );
    if(ZSuccess == status)
    {
      cnfParam->cmd = cmd;
      cnfParam->nodeId = dest;
      *handle = cnfParam->handle;
    }
    else
    {
      MFG_free(cnfParam);
    }
  }
  return (uint8)status;
}

void cmdZdp_LeaveRsp(uint16 nwkAddr, uint8 seqNum, uint8 status)
{
  if(cmdZdp_SeqCmp(seqNum))
  {
    cmdZdp_CmdOutput( (ZDP_LEAVE | 0x8000), nwkAddr, status, seqNum, 0, NULL );
  }
}
